#include "cartographer/rcm/imu/integrate.h"

namespace cartographer
{
    ImuMiddleCalculator::ImuMiddleCalculator(const common::Time &timeStart, const common::Time &timeEnd, int dataSize)
        : time_end(timeEnd),
          time_start(timeStart),
          data_size(dataSize)
    {
    }

    int ImuMiddleCalculator::Calculate(const ImuDataAlias &imuCurr, const ImuDataAlias &imuNext, int idx)
    {
        LOG(INFO) << "ImuPreintegrate before mid [" << idx << "]\n"
                  << "imuCurr:\n"
                  << "acc:" << VecToStr(imuCurr.a) << "\n"
                  << "ang:" << VecToStr(imuCurr.w) << "\n"
                  << "time:" << common::ToUniversal(imuCurr.time) << "\n"
                  << "imuNext:\n"
                  << "acc:" << VecToStr(imuNext.a) << "\n"
                  << "ang:" << VecToStr(imuNext.w) << "\n"
                  << "time:" << common::ToUniversal(imuNext.time) << "\n";

        if (HasNan(imuNext.a) || HasNan(imuNext.w))
        {
            LOG(INFO) << "HasNan-------------\n";
            return -1;
        }
        if ((idx == 0) && (idx < (data_size - 1)))
        {
            // 边缘部分线性插值, 第一个但不是最后一个IMU，即其后面还有IMU数据
            // 两关键帧(F0,F1)间有若干(不少于2)IMU数据，F0的时间是time_start，F1的时间是time_end，以加速度为例，加
            // 速度依次为A0,A1,A2，由于不同传感器数据时间顺序不确定，所以时间上A0可能早于F0，也可能晚于F0，要求F0时
            // 刻的加速度就要做加速度补偿，即求F0时刻与A0
            // 时刻的加速度变化量，然后用A0加或者减去加速度变化量即得F0时刻的加速度，这个加速度变化量只能估计，可用A1
            // 和A0的差值除以A1和A0的时间差，得到A1和A0时刻间的加速度变化速率，用加速度变化率乘以A0到F0的时间差即可
            // 得这个加速补偿值(可正可负)，用A0加上补偿值求得F0时刻的加速，用F0时刻的加速与A1的加速求平均得到中值加速
            // 用这个中值加速进行IMU预积分
            float tab = common::ToSeconds(imuNext.time - imuCurr.time);
            float tini = common::ToSeconds(imuCurr.time - time_start);
            acc = (imuCurr.a - (imuNext.a - imuCurr.a) * (tini / tab) + imuNext.a) * 0.5f;
            angular = (imuCurr.w - (imuNext.w - imuCurr.w) * (tini / tab) + imuNext.w) * 0.5f;
            tstep = common::ToSeconds(imuNext.time - time_start);
            LOG(INFO) << "tab " << tab << " tini " << tini << "\n";
        }
        else if (idx < (data_size - 1))
        {
            // 中间的IMU数据直接求平均
            acc = (imuCurr.a + imuNext.a) * 0.5f;
            angular = (imuCurr.w + imuNext.w) * 0.5f;
            tstep = common::ToSeconds(imuNext.time - imuCurr.time);
        }
        else if ((idx > 0) && (idx == (data_size - 1)))
        {
            // 最后一个IMU数据，同理补偿求F1时刻的加速度和角速度
            float tab = common::ToSeconds(imuNext.time - imuCurr.time);
            float tend = common::ToSeconds(imuNext.time - time_end);
            acc = (imuCurr.a + imuNext.a - (imuNext.a - imuCurr.a) * (tend / tab)) * 0.5f;
            angular = (imuCurr.w + imuNext.w - (imuNext.w - imuCurr.w) * (tend / tab)) * 0.5f;
            tstep = common::ToSeconds(time_end - imuCurr.time);
            LOG(INFO) << "tab " << tab << " tend " << tend << "\n";
        }
        else if ((idx == 0) && (idx == (data_size - 1)))
        {
            // 两个关键帧间只有两个IMU数据，不做补偿
            acc = imuCurr.a;
            angular = imuCurr.w;
            tstep = common::ToSeconds(time_end - time_start);
        }
        LOG(INFO) << "mid res: acc:" << VecToStr(acc) << " "
                  << "ang:" << VecToStr(angular) << " "
                  << "time:" << tstep << "\n";
        return 1;
    }

} // namespace cartographer